#include "LegacyMotion.h"
#include "../../RapidXML/rapidxml.hpp"
#include "../../XMLTools.h"
#include "MMM/MathTools.h"
#include <filesystem>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/LU>

using namespace boost::accumulators;

using std::cout;
using std::endl;


namespace MMM
{

LegacyMotion::LegacyMotion (const std::string& name)
:name(name)
{
}

LegacyMotion::LegacyMotion (const LegacyMotion& m):
    motionFrames(m.motionFrames),
    jointNames(m.jointNames),
    name(m.name),
    model(m.model),
    originalModel(m.originalModel),
    modelProcessor(m.modelProcessor),
    motionFilePath(m.motionFilePath),
    motionFileName(m.motionFileName),
    modelFilePath(m.modelFilePath)
{
}

LegacyMotionPtr LegacyMotion::copy(){
    LegacyMotionPtr m(new LegacyMotion("copy"));
    m->motionFrames.clear();
    for(auto & motionFrame : this->motionFrames){
        MotionFramePtr mfp = motionFrame->copy();
        m->motionFrames.push_back(mfp);
    }

    m->jointNames = this->jointNames;
    m->name = this->name;
    m->model = this->model;
    m->originalModel = this->originalModel;
    m->modelProcessor = this->modelProcessor;
    m->modelFilePath = this->modelFilePath;

    return m;
}

bool LegacyMotion::addMotionFrame(MotionFramePtr md)
{
	if (!md)
		return false;
	if (jointNames.size()>0 && md->joint.rows()>0)
        if (static_cast<std::size_t>(md->joint.rows()) != jointNames.size())
		{
			MMM_ERROR << "Error: md.joint.rows()=" << md->joint.rows() << ", but jointNames.size()=" << jointNames.size() << endl;
			return false;
		}
	motionFrames.push_back(md);
    return true;
}

bool LegacyMotion::removeMotionFrame(size_t frame)
{
    if(frame >= motionFrames.size())
        return false;
    motionFrames.erase(motionFrames.begin()+frame);
    return true;
}

bool LegacyMotion::setJointOrder(const std::vector<std::string> &jointNames)
{
	if (jointNames.size()==0)
		return false;
	this->jointNames = jointNames;
	return true;
}

void LegacyMotion::setComment(const std::string &comment)
{
	CommentEntryPtr c(new CommentEntry());
	c->comments.push_back(comment);
	addEntry("comments",c);
}


void LegacyMotion::addComment(const std::string &comment)
{
    if (!hasEntry("comments"))
        setComment(comment);
    else
    {
        MotionEntryPtr c1 = getEntry("comments");
        CommentEntryPtr c = boost::dynamic_pointer_cast<CommentEntry>(c1);
        if (c)
            c->comments.push_back(comment);
    }
}

void LegacyMotion::setModel(ModelPtr model)
{
	this->model = model;
	this->originalModel = model;
}

void LegacyMotion::setModel(ModelPtr processedModel, ModelPtr originalModel)
{
	this->model = processedModel;
    this->originalModel = originalModel;
}

ModelPtr LegacyMotion::getModel(bool processedModel)
{
    if(processedModel)
        return model;
    else
        return originalModel;
}

const std::string &LegacyMotion::getMotionFilePath()
{
    return motionFilePath;
}

const std::string &LegacyMotion::getMotionFileName()
{
    return motionFileName;
}

void LegacyMotion::setMotionFilePath(const std::string &filepath)
{
    this->motionFilePath = filepath;
}

void LegacyMotion::setMotionFileName(const std::string &filename)
{
    this->motionFileName = filename;
}

const std::string &LegacyMotion::getModelFilePath()
{
    return modelFilePath;
}

void LegacyMotion::setModelFilePath(const std::string &filepath)
{
    this->modelFilePath = filepath;
}

void LegacyMotion::setModelProcessor(ModelProcessorPtr mp)
{
    this->modelProcessor = mp;
}

ModelProcessorPtr LegacyMotion::getModelProcessor()
{
    return modelProcessor;
}

std::string LegacyMotion::toXML()
{
	std::string tab1 = "\t";
	std::string tab2 = "\t\t";
	std::string tab3 = "\t\t\t";
	std::stringstream res;
	//res << "<? xml version='1.0' ?>" << endl;

    res << tab1 << "<Motion name='" << name << "'>" << endl;
	std::map<std::string, MotionEntryPtr>::iterator i = motionEntries.begin();
	while (i != motionEntries.end())
	{
		res << i->second->toXML();
		i++;
	}
	if (originalModel && !originalModel->getFilename().empty())
	{
		std::string modelFile = originalModel->getFilename();
		if (!motionFilePath.empty())
		{
			// make relative path
            MMM::XML::makeRelativePath(motionFilePath, modelFile);
			//modelFile = MMM::XML::make_relative(motionFilePath, modelFile);
		}

		res << tab2 << "<Model>" << endl;
		res << tab3 << "<File>" << modelFile << "</File>" << endl;
		res << tab2 << "</Model>" << endl;
	}
	if (modelProcessor)
	{
		res << modelProcessor->toXML(2);
	}

	if (jointNames.size()>0)
	{
		res << tab2 << "<JointOrder>" << endl;
		for (const auto & jointName : jointNames)
		{
			res << tab3 << "<Joint name='" << jointName << "'/>" << endl;
		}
		res << tab2 << "</JointOrder>" << endl;
	}
	res << tab2 << "<MotionFrames>" << endl;		
	for (auto & motionFrame : motionFrames)
	{
		res << motionFrame->toXML(); 
	}
	res << tab2 << "</MotionFrames>" << endl;
	res << tab1 << "</Motion>" << endl;
	return res.str();
}

void  LegacyMotion::print()
{
	std::string s = toXML();
	cout << s;
}

void LegacyMotion::print(const std::string &filename){
    std::ofstream ofs(filename.c_str());
    std::string s = toXML();
    ofs << "<?xml version='1.0'?>\n";
    ofs << "<MMM>\n";
    ofs << s;
    ofs << "</MMM>\n";
    ofs.close();
}

void LegacyMotion::outputData(const std::string &filename)
{
    std::ofstream ofs(filename.c_str());

    for(size_t i = 0; i < getMotionFrames().size(); i++){
        MotionFramePtr frame = getMotionFrame(i);
        Eigen::Vector3f pos = frame->getRootPos();

        for(int j= 0; j < pos.rows(); j++){
            ofs << pos[j];
            ofs << '\t';
        }

        ofs << '\n';

    }
    ofs.close();
}


std::vector<MotionFramePtr> LegacyMotion::getMotionFrames()
{
	return motionFrames;
}

MotionFramePtr LegacyMotion::getMotionFrame(size_t frame)
{
	if (frame>=motionFrames.size())
	{
		MMM_ERROR << "OutOfBounds error in getMotionFrame:" << frame << endl;
		return MotionFramePtr();
	}
	return motionFrames[frame];
}

std::vector<std::string> LegacyMotion::getJointNames()
{
	return jointNames;
}

unsigned int LegacyMotion::getNumFrames()
{
	return (unsigned int)motionFrames.size();
}

std::string LegacyMotion::getComment()
{
	if (!hasEntry("comments"))
		return std::string();
	MotionEntryPtr e = getEntry("comments");
	CommentEntryPtr r = boost::dynamic_pointer_cast<CommentEntry>(e);
	if (!r)
		return std::string();
	return r->getCommentString();
}

void LegacyMotion::setName(const std::string& name) {
    this->name = name;
}

std::string LegacyMotion::getName() {
    return name;
}


bool LegacyMotion::hasJoint(const std::string& name)
{
	std::string lc = name;
	XML::toLowerCase(lc);
	for(const auto & jointName : jointNames)
	{
		if(jointName==name)
		{
			return true;
		}
	}
	return false;
}

void LegacyMotion::calculateVelocities(int method)
{
    std::vector<MotionFramePtr> frames = getMotionFrames();
    Eigen::MatrixXf jointValues = getJointValuesAsMatrix();
    Eigen::MatrixXf jointVelocities = calculateDifferentialQuotient(jointValues,method);

    for (size_t i=0; i < frames.size(); i++)
    {
        frames[i]->joint_vel = jointVelocities.row(i);
    }

    return;
}

void LegacyMotion::calculateAccelerations(int method)
{
    std::vector<MotionFramePtr> frames = getMotionFrames();
    Eigen::MatrixXf jointVelocities = getJointVelocitiesAsMatrix();
    Eigen::MatrixXf jointAcclerations = calculateDifferentialQuotient(jointVelocities,method);

    for (size_t i=0; i < frames.size(); i++)
    {
        frames[i]->joint_acc = jointAcclerations.row(i);
    }

    return;
}

void LegacyMotion::smoothJointValues(jointEnum type, int windowSize)
{
    if (type == LegacyMotion::eValues)
    {
        Eigen::MatrixXf input = getJointValuesAsMatrix();

        for (int i=0; i < input.cols(); i++)
        {
            Eigen::VectorXf cv = input.col(i);
            Eigen::VectorXf smoothed_cv = applyRollingMeanSmoothing(cv, windowSize);

            // now, write back smoothed values
            for(size_t j=0; j < motionFrames.size(); j++)
            {
                motionFrames[j]->joint(i) = smoothed_cv(j);
            }

        }

    }
    else if (type == LegacyMotion::eVelocities)
    {
        Eigen::MatrixXf input = getJointVelocitiesAsMatrix();

        for (int i=0; i < input.cols(); i++)
        {
            Eigen::VectorXf cv = input.col(i);
            Eigen::VectorXf smoothed_cv = applyRollingMeanSmoothing(cv, windowSize);

            // now, write back smoothed values
            for(size_t j=0; j < motionFrames.size(); j++)
            {
                motionFrames[j]->joint_vel(i) = smoothed_cv(j);
            }

        }
    }
    else if (type == LegacyMotion::eAccelerations)
    {
        Eigen::MatrixXf input = getJointAccelerationsAsMatrix();

        for (int i=0; i < input.cols(); i++)
        {
            Eigen::VectorXf cv = input.col(i);
            Eigen::VectorXf smoothed_cv = applyRollingMeanSmoothing(cv, windowSize);

            // now, write back smoothed values
            for(size_t j=0; j < motionFrames.size(); j++)
            {
                motionFrames[j]->joint_acc(i) = smoothed_cv(j);
            }

        }
    }
}



Eigen::MatrixXf LegacyMotion::getJointValuesAsMatrix()
{
    std::vector<MotionFramePtr> frames = getMotionFrames();
    int nFrames = frames.size();
    int nDof = frames[0]->joint.rows();

    Eigen::MatrixXf jointValues = Eigen::MatrixXf::Zero(nFrames, nDof);

    for (int i=0; i < nFrames; i++)
    {
        jointValues.row(i) = frames[i]->joint;
    }

    return jointValues;
}


Eigen::MatrixXf LegacyMotion::getJointVelocitiesAsMatrix()
{
    std::vector<MotionFramePtr> frames = getMotionFrames();
    int nFrames = frames.size();
    int nDof = frames[0]->joint.rows();

    Eigen::MatrixXf jointVelocities = Eigen::MatrixXf::Zero(nFrames,nDof);

    for (int i=0; i < nFrames; i++)
    {
        jointVelocities.row(i) = frames[i]->joint_vel;
    }

    return jointVelocities;
}
Eigen::MatrixXf LegacyMotion::getJointAccelerationsAsMatrix()
{
    std::vector<MotionFramePtr> frames = getMotionFrames();
    int nFrames = frames.size();
    int nDof = frames[0]->joint.rows();

    Eigen::MatrixXf jointAccelerations = Eigen::MatrixXf::Zero(nFrames,nDof);

    for (int i=0; i < nFrames; i++)
    {
        jointAccelerations.row(i) = frames[i]->joint_acc;
    }

    return jointAccelerations;
}






Eigen::MatrixXf LegacyMotion::calculateDifferentialQuotient(const Eigen::MatrixXf &inputMatrix, int method)
{
    Eigen::MatrixXf outputMatrix = Eigen::MatrixXf(inputMatrix.rows(),inputMatrix.cols());

    if (method == 0) // centralized
    {
        Eigen::VectorXf x1 = Eigen::VectorXf(inputMatrix.cols());
        Eigen::VectorXf x2 = Eigen::VectorXf(inputMatrix.cols());


        //iterate over all frames
        for (int i=0; i< inputMatrix.rows(); i++)
        {
            if (i==0)
                x1 = inputMatrix.row(0);
            else
                x1 = inputMatrix.row(i-1);

            if(i==inputMatrix.rows()-1)
                x2 = inputMatrix.row(inputMatrix.rows()-1);
            else
                x2 = inputMatrix.row(i+1);

            outputMatrix.row(i) = (x2-x1)/float(2.0*0.01);
        }

    }

    return outputMatrix;
}

Eigen::VectorXf LegacyMotion::applyRollingMeanSmoothing(Eigen::VectorXf &input, int windowSize)
{
    Eigen::VectorXf output = Eigen::VectorXf::Zero(input.rows());
    accumulator_set<float, stats<tag::rolling_mean> > acc(tag::rolling_window::window_size = windowSize);

    for(int i=0; i < input.rows();i++)
    {
        acc(input(i));
        output(i) = rolling_mean(acc);
    }

    return output;
}



LegacyMotionPtr LegacyMotion::getSegmentMotion(size_t frame1, size_t frame2){
    if(frame1 > frame2){
        size_t tframe = frame1;
        frame1 = frame2;
        frame2 = tframe;
    }

    LegacyMotionPtr res(new LegacyMotion("na"));
    res = copy();
    std::string cname = getName() + "_segmented";
    res->setName(cname);

    if(frame2 > motionFrames.size()){
        return res;
    }

    std::vector<MotionFramePtr>::iterator begin = motionFrames.begin() + frame1;
    std::vector<MotionFramePtr>::iterator end = motionFrames.begin() + frame2;

    std::vector<MotionFramePtr> newMotionFrames(begin,end);

    res->setMotionFrames(newMotionFrames);

    res->setModel(getModel());
    res->setModelProcessor(getModelProcessor());
    return res;
}


LegacyMotionList LegacyMotion::getSegmentMotions(std::vector<int> segmentPoints){
    LegacyMotionList res;

    std::vector<int>::iterator it = segmentPoints.begin();
    segmentPoints.insert(it, 0);
    segmentPoints.push_back(getNumFrames());
    it = segmentPoints.begin();

    for(size_t i = 0; i < segmentPoints.size()-1; i++){
        std::ostringstream ss;
        ss << i;
        std::string segMotionName = getName() + "_" + "Segment" + ss.str();
        LegacyMotionPtr segMotion = getSegmentMotion(segmentPoints[i], segmentPoints[i+1]);
        segMotion->setName(segMotionName);
        segMotion->setModel(getModel());
        segMotion->setModelProcessor(getModelProcessor());
        res.push_back(segMotion);
    }

    return res;
}

void LegacyMotion::joinMotion(LegacyMotionPtr next)
{
    MotionFramePtr lastFrame = getMotionFrame(getMotionFrames().size()-1);
    float timestep = lastFrame->timestep;

    Eigen::Matrix4f basePose = lastFrame->getRootPose();

    LegacyMotionPtr nextCopy = next->copy();
    nextCopy->setStartPose(basePose);

    for(std::size_t i = 0; i < nextCopy->getMotionFrames().size(); i++)
    {
        MotionFramePtr frame = nextCopy->getMotionFrame(i)->copy();
        frame->timestep += timestep;
        addMotionFrame(frame);
    }


}

void LegacyMotion::setStartPosition(const Eigen::Vector3f &startPosition)
{
    MotionFramePtr startFrame = getMotionFrame(0);
    Eigen::Vector3f basePos = startFrame->getRootPos();
    Eigen::Vector3f diffPos = basePos - startPosition;

    for(size_t i = 0; i < getMotionFrames().size(); i++){
        MotionFramePtr curframe = getMotionFrame(i);
        basePos = curframe->getRootPos();
        curframe->setRootPos(basePos - diffPos);
    }
}

void LegacyMotion::setStartPose(const Eigen::Matrix4f & startPose)
{
    MotionFramePtr startFrame = getMotionFrame(0);
    Eigen::Matrix4f basePose = startFrame->getRootPose();
    Eigen::Matrix4f diffPose = startPose * basePose.inverse();

    for(size_t i = 0; i < getMotionFrames().size(); i++) {
        MotionFramePtr curframe = getMotionFrame(i);
        basePose = diffPose * curframe->getRootPose();

        curframe->setRootPose(basePose);
    }

}

std::vector<double> LegacyMotion::getTimestamps()
{
    std::vector<double> res;
    for(size_t i = 0; i < getMotionFrames().size(); i++){
        MotionFramePtr curframe = getMotionFrame(i);
        res.push_back(curframe->timestep);
    }

    return res;
}

void LegacyMotion::setStartTime(double start, double step)
{
    double curTime = start;
    for(size_t i = 0; i < getNumFrames(); i++)
    {
        MotionFramePtr curframe = getMotionFrame(i);
        curframe->timestep = curTime;
        curTime += step;
    }
}

Eigen::MatrixXf LegacyMotion::calculateDerivativeOverSpline(const Eigen::MatrixXf &inputMatrix, bool firstOrder)
{
    Eigen::MatrixXf outputMatrix = Eigen::MatrixXf(inputMatrix.rows(),inputMatrix.cols());

#ifdef ALGLIB_AVAILABLE
    for (int i =  0; i < inputMatrix.cols(); i++){

        Math::SplineFunction s(inputMatrix.col(i));
        Eigen::VectorXf derivatives = s.derivative;

        if (firstOrder == false){
            derivatives = s.derivative2ndOrder;
        }
        for (int j = 0; j < derivatives.rows(); j++) {
            outputMatrix(j,i) = derivatives(j);
        }
    }
#else
    MMM_ERROR << "Cannot use spline derivation: MMMCore has been compiled without support for ALGLIB!" << std::endl;
#endif

    return outputMatrix;
}

}//namespace


